#ifndef _ROBOT_H_
#define _ROBOT_H_

#include "common.h"
#include "keya.h"
#include "RobotConfig.h"


//机器人操作指令集结构体---全部是函数指针---void指针仿 C++ 中隐含的This指针
typedef struct
{
	void (*Init)(void *this); //初始化关节--模式配置与参数加载
	// void (*ClearERR)(void *this); //清除错误--主要是驱动器
	void (*Run)(void *this);  //让关节运动，如何运动取决于参数
	void (*Stop)(void *this); //使关节停止
	void (*SetVelocity)(void *this); //设置速度s
	// void (*SetPosition)(void *this);//设置目标位置
	// void (*SetActive)(void *this); //使能设置
	// void (*SetCtrlMode)(void *this); //控制模式设置
	void (*GetCurrentNow)(void *this);//获取电流
	void (*GetVelocityNow)(void *this); //获取速度
	void (*GetPositionNow)(void *this);//获取目标位置

}DriveOps_t;


//机器人关节结构体
typedef struct
{	
	//属性
	uint8_t	Vendor;		//驱动器厂商标识,用于区分驱动器和分配操作指令集
	uint8_t	Status;		//运行状态： 运行-停止
	MOTORCTRLMODE_t	CtrlMode;			//运行模式： 无模式，速度，位置，电流，回零等
	uint32_t 	ServoAngleNow;	//关节角Now
	uint32_t 	ServoAngleTarget;	//关节角Target
	uint32_t 	ServoVelocity;	//关节运动速度
	uint8_t	Event;		//事件： 开关事件，电流限制，其他异常
	uint8_t	CANID;		//0:非can设备，其他can节点号
	uint8_t	Channel;	//0:不区分通道，其他channel1 ，channel2
	//方法
	DriveOps_t 	*ops;  //操作集的指针，初始化时根据vendor指向不同集合
}RobotServo_t;

extern RobotServo_t RobotServo[USE_MOTOR_NUM];  //保存电机信息

void RobotServoGetVelocityNow(RobotServo_t *Servo);
void RobotServoGetCurrentNow(RobotServo_t *Servo);
void RobotServoGetPositionNow(RobotServo_t *Servo);


void RobotServoCtrlProcess(uint8_t devicetype, uint8_t name, uint32_t value);
void RobotSystemInfoInit(void);
void RobotServoAllInit(void);
void RobotActionCTRL(ACTION_TYPE act_num, int32_t value);
void RobotEachControl(int32_t speed_r, int32_t speed_l);
void Robot_PID_Ctrl(int32_t speed, int16_t off);

#endif // !_ROBOT_H_
